/******************************************************************************/
/*  mb_func_coils.c                                                           */
/*                                                                            */
/*  FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU. */
/*  Copyright (c) 2006 Christian Walter <wolti@sil.at>                        */
/*  All rights reserved.                                                      */
/******************************************************************************/

#include "data_types.h"

#include "port\port.h"
#include "mbconfig.h"
#include "mb.h"
#include "mbconfig.h"
#include "mbframe.h"
#include "mbproto.h"
#include "functions\mbutils.h"
#include "mbfunc.h"



/******************************************************************************/
/*      Defines                                                               */
/******************************************************************************/
#define MB_PDU_FUNC_READ_ADDR_OFF           (MB_PDU_DATA_OFF)
#define MB_PDU_FUNC_READ_COILCNT_OFF        (MB_PDU_DATA_OFF + 2)
#define MB_PDU_FUNC_READ_SIZE               (4)
#define MB_PDU_FUNC_READ_COILCNT_MAX        (0x07D0)

#define MB_PDU_FUNC_WRITE_ADDR_OFF          (MB_PDU_DATA_OFF)
#define MB_PDU_FUNC_WRITE_VALUE_OFF         (MB_PDU_DATA_OFF + 2)
#define MB_PDU_FUNC_WRITE_SIZE              (4)

#define MB_PDU_FUNC_WRITE_MUL_ADDR_OFF      (MB_PDU_DATA_OFF)
#define MB_PDU_FUNC_WRITE_MUL_COILCNT_OFF   (MB_PDU_DATA_OFF + 2)
#define MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF   (MB_PDU_DATA_OFF + 4)
#define MB_PDU_FUNC_WRITE_MUL_VALUES_OFF    (MB_PDU_DATA_OFF + 5)
#define MB_PDU_FUNC_WRITE_MUL_SIZE_MIN      (5)
#define MB_PDU_FUNC_WRITE_MUL_COILCNT_MAX   (0x07B0)




eMB_EXCEPTION prveMbError2Exception(const eMB_ERROR_CODE _errorCode);




#if MB_FUNC_READ_COILS_ENABLED > 0
//==============================================================================
// MbFuncReadCoils()
//==============================================================================
eMB_EXCEPTION MbFuncReadCoils(uint8_t * _pFrame, uint16_t * _pLen)
{
    uint16_t regAddress;
    uint16_t coilCount;
    uint8_t numBytes;
    uint8_t * pFrameCur;

    eMB_EXCEPTION status = MB_EX_NONE;
    eMB_ERROR_CODE regStatus;

    if(*_pLen == (MB_PDU_FUNC_READ_SIZE + MB_PDU_SIZE_MIN)) {
        regAddress = (uint16_t)(_pFrame[MB_PDU_FUNC_READ_ADDR_OFF] << 8);
        regAddress |= (uint16_t)(_pFrame[MB_PDU_FUNC_READ_ADDR_OFF + 1]);
        regAddress++;

        coilCount = (uint16_t)(_pFrame[MB_PDU_FUNC_READ_COILCNT_OFF] << 8);
        coilCount |= (uint16_t)(_pFrame[MB_PDU_FUNC_READ_COILCNT_OFF + 1]);

        // Check if the number of registers to read is valid. If not
        // return Modbus illegal data value exception.
        if((coilCount >= 1) && (coilCount < MB_PDU_FUNC_READ_COILCNT_MAX)) {
            // Set the current PDU data pointer to the beginning
            pFrameCur = &_pFrame[MB_PDU_FUNC_OFF];
            *_pLen = MB_PDU_FUNC_OFF;

            // First byte contains the function code
            *pFrameCur++ = MB_FUNC_READ_COILS;
            *_pLen += 1;

            // Test if the quantity of coils is a multiple of 8. If not last
            // byte is only partially field with unused coils set to zero.
            if((coilCount & 0x0007) != 0) {
                numBytes = (uint8_t)(coilCount / 8 + 1);
            }
            else {
                numBytes = (uint8_t)(coilCount / 8);
            };

            *pFrameCur++ = numBytes;
            *_pLen += 1;

            regStatus = ModbusRegCoilsCb(pFrameCur, regAddress, coilCount, MB_REG_READ);

            // If an error occured convert it into a Modbus exception
            if(regStatus != MB_ENOERR) {
                status = prveMbError2Exception(regStatus);
            }
            else {
                /*  The response contains the function code, the starting address
                 *  and the quantity of registers. We reuse the old values in the 
                 *  buffer because they are still valid.
                 */
                *_pLen += numBytes;;
            };
        }
        else {
            status = MB_EX_ILLEGAL_DATA_VALUE;
        };
    }
    else {
        // Can't be a valid read coil register request because the length
        // is incorrect
        status = MB_EX_ILLEGAL_DATA_VALUE;
    };

    return status;
}
//==============================================================================
//==============================================================================
#endif


#if MB_FUNC_WRITE_COIL_ENABLED > 0
//==============================================================================
// MbFuncWriteCoil()
//==============================================================================
eMB_EXCEPTION MbFuncWriteCoil(uint8_t * _pFrame, uint16_t * _pLen)
{
    uint16_t regAddress;
    uint8_t buf[2];

    eMB_EXCEPTION status = MB_EX_NONE;
    eMB_ERROR_CODE regStatus;

    if(*_pLen == (MB_PDU_FUNC_WRITE_SIZE + MB_PDU_SIZE_MIN)) {
        regAddress = (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_ADDR_OFF] << 8);
        regAddress |= (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_ADDR_OFF + 1]);
        regAddress++;

        if((_pFrame[MB_PDU_FUNC_WRITE_VALUE_OFF + 1] == 0x00)
            && ((_pFrame[MB_PDU_FUNC_WRITE_VALUE_OFF] == 0xFF)
                || (_pFrame[MB_PDU_FUNC_WRITE_VALUE_OFF] == 0x00)))
        {
            buf[1] = 0;
            if(_pFrame[MB_PDU_FUNC_WRITE_VALUE_OFF] == 0xFF) {
                buf[0] = 1;
            }
            else {
                buf[0] = 0;
            };

            regStatus = ModbusRegCoilsCb(buf, regAddress, 1, MB_REG_WRITE);

            // If an error occured convert it into a Modbus exception
            if(regStatus != MB_ENOERR) {
                status = prveMbError2Exception(regStatus);
            };
        }
        else {
            status = MB_EX_ILLEGAL_DATA_VALUE;
        };
    }
    else {
        // Can't be a valid write coil register request because the length
        // is incorrect.
        status = MB_EX_ILLEGAL_DATA_VALUE;
    };

    return status;
}
//==============================================================================
//==============================================================================
#endif


#if MB_FUNC_WRITE_MULTIPLE_COILS_ENABLED > 0
//==============================================================================
// ModbusInit()
//==============================================================================
eMB_EXCEPTION MbFuncWriteMultipleCoils(uint8_t * _pFrame, uint16_t * _pLen)
{
    uint16_t regAddress;
    uint16_t coilCount;
    uint8_t byteCount;
    uint8_t byteCountVerify;

    eMB_EXCEPTION status = MB_EX_NONE;
    eMB_ERROR_CODE regStatus;

    if(*_pLen > (MB_PDU_FUNC_WRITE_SIZE + MB_PDU_SIZE_MIN)) {
        regAddress = (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF] << 8);
        regAddress |= (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF + 1]);
        regAddress++;

        coilCount = (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_MUL_COILCNT_OFF] << 8);
        coilCount |= (uint16_t)(_pFrame[MB_PDU_FUNC_WRITE_MUL_COILCNT_OFF + 1]);

        byteCount = _pFrame[MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF];

        // Compute the number of expected bytes in the request
        if((coilCount & 0x0007) != 0) {
            byteCountVerify = (uint8_t)(coilCount / 8 + 1);
        }
        else {
            byteCountVerify = (uint8_t)(coilCount / 8);
        };

        if((coilCount >= 1)
            && (coilCount <= MB_PDU_FUNC_WRITE_MUL_COILCNT_MAX)
                && (byteCountVerify == byteCount))
        {
            regStatus = ModbusRegCoilsCb(&_pFrame[MB_PDU_FUNC_WRITE_MUL_VALUES_OFF],
                                         regAddress, coilCount, MB_REG_WRITE);

            // If an error occured convert it into a Modbus exception
            if(regStatus != MB_ENOERR) {
                status = prveMbError2Exception(regStatus);
            }
            else {
                /*  The response contains the function code, the starting address
                 *  and the quantity of registers. We reuse the old values in the 
                 *  buffer because they are still valid.
                 */
                *_pLen = MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF;
            };
        }
        else {
            status = MB_EX_ILLEGAL_DATA_VALUE;
        };
    }
    else {
        // Can't be a valid write coil register request because the length
        // is incorrect.
        status = MB_EX_ILLEGAL_DATA_VALUE;
    };

    return status;
}
//==============================================================================
//==============================================================================
#endif